#!/usr/bin/env python

# Configuration
NODE_NAME = 'filtered_face_detector'
CASCADE_FILE_PATH = 'data/haarcascade_frontalface_alt.xml'

# -------
# Imports:
# -------
# Python
import threading

# ROS
import roslib; roslib.load_manifest(NODE_NAME)
import rospy
from geometric_image_filter.msg import FilteredImage
from sensor_msgs.msg import Image
import cv
from cv_bridge import CvBridge

# ------------------------------------------
class FilteredFaceDetector(threading.Thread):
# ------------------------------------------
	# Members:
	cascade = None
	bridge = None
	imageMessage = None

	# ---------------------------------
	def __init__(self, cascadeFilePath):
	# ---------------------------------
		# Initialize our node
		rospy.init_node(NODE_NAME)

		# Load the classifier
		self.cascade = cv.Load(cascadeFilePath)

		# Ros image < -- > OpenCV Image
		self.bridge = CvBridge()

		# Subscribe to the image topic
		rospy.Subscriber('image', Image, self.__imageCallback)
	
	# -----------
	def run(self):
	# -----------
		from time import time
		times = []

		# Main control loop
		while not rospy.is_shutdown():
			if self.imageMessage != None:
				# Alias the image message and convert it to an OpenCV image
				im = self.imageMessage
				cvIm = self.bridge.imgmsg_to_cv(im, 'mono8')

				start = time()				
				rospy.loginfo(cv.HaarDetectObjects(cvIm, self.cascade, cv.CreateMemStorage(0), 1.2, 2, 0, (20, 20)))
				duration = time() - start			
				times.append(duration)

		averageTime = float(sum(times)) / len(times)
		rospy.loginfo('Total frames: %i', len(times))
		rospy.loginfo('Average frame duration: %f', averageTime)

	# ---------
	# Callbacks|
	# ----------------------------
	def __imageCallback(self, msg):
	# ----------------------------
		self.imageMessage = msg

# -----------------------
if __name__ == '__main__':
# -----------------------
	controller = FilteredFaceDetector(CASCADE_FILE_PATH)
	controller.run()

